#include "../include/bambu.hpp"
#include "../include/my_fs.hpp"
#include "../include/motor.hpp"
#include "../include/my_web.hpp"
#include "../include/debug_info.hpp"
#define BAMBU_DEBUG 1
#define DBG_BAMBU(x) do{\
                         if(BAMBU_DEBUG){\
                           Serial.print(String("BAMBU_DEBUG:") + x);\
                           dbg.ops->send(String("BAMBU_DEBUG:") + x);\
                           my_web.ops->addLog(x);\
                           }\
                       }while(0)
#define HOMEING_TIME 40

WiFiClientSecure bambuWifiClient;
PubSubClient bambuClient(bambuWifiClient);

#define RECONNECT_TIMES 10

static BAMBU_DATA data;

static BAMBU_COMMAND command = {
    .resume     = "{\"print\":{\"command\":\"resume\",\"sequence_id\":\"1\"},\"user_id\":\"1\"}",
    .unload     = "{\"print\":{\"command\":\"ams_change_filament\",\"curr_temp\":220,\"sequence_id\":\"1\",\"tar_temp\":220,\"target\":255},\"user_id\":\"1\"}",
    .load       = "{\"print\":{\"command\":\"ams_change_filament\",\"curr_temp\":220,\"sequence_id\":\"1\",\"tar_temp\":220,\"target\":254},\"user_id\":\"1\"}",
    .done       = "{\"print\":{\"command\":\"ams_control\",\"param\":\"done\",\"sequence_id\":\"1\"},\"user_id\":\"1\"}",
    .pause      = "{\"print\":{\"command\":\"ams_control\",\"param\":\"pause\",\"sequence_id\":\"1\"}}",
    .clear      = "{\"print\":{\"command\": \"clean_print_error\",\"sequence_id\":\"1\"},\"user_id\":\"1\"}",
    .status     = "{\"pushing\": {\"sequence_id\": \"0\", \"command\": \"pushall\"}}",
    .ledON      = "{\"system\":{\"sequence_id\":\"0\",\"command\":\"ledctrl\",\"led_node\":\"chamber_light\",\"led_mode\":\"on\"}}",
    .ledOFF     = "{\"system\":{\"sequence_id\":\"0\",\"command\":\"ledctrl\",\"led_node\":\"chamber_light\",\"led_mode\":\"off\"}}",
    };

static BAMBU_CFG cfg = {
    .mqtt_user  = "bblp",
    .mqtt_id    = "ams",
    .RenewTime  = 1000,
    .led        = false,
};

static void connectBambuMQTT() {
    static int count = 0;
    static bool first_boot = true;
    if(first_boot){
        motor.ops->keepLow();
        bambuWifiClient.setInsecure();
        bambuClient.setServer(bambu.cfg->mqtt_broker.c_str(), 8883);
        bambuClient.setCallback(bambu.cfg->callback);
        bambu.cfg->topic_subscribe  = "device/" + String(bambu.cfg->device_serial) + "/report";
        bambu.cfg->topic_publish    = "device/" + String(bambu.cfg->device_serial) + "/request";
        delay(1000);
    }
__bambu_reconnect__:
    if(!bambuClient.connected()) {
        count ++;
        bambu.cfg->mqtt_id = "AMS-"+String(millis());
        DBG_BAMBU("尝试连接拓竹mqtt|"+bambu.cfg->mqtt_id+"|"+bambu.cfg->mqtt_user+"|"+bambu.cfg->mqtt_password);
        if (bambuClient.connect(bambu.cfg->mqtt_id.c_str(), bambu.cfg->mqtt_user.c_str(), bambu.cfg->mqtt_password.c_str())) {
            count = 0;
            first_boot = false;
            motor.ops->init();
            DBG_BAMBU("连接成功!");
            bambuClient.subscribe(bambu.cfg->topic_subscribe.c_str());
        }
        else {
            DBG_BAMBU("连接失败，失败原因:"+bambuClient.state());
            DBG_BAMBU("5秒后尝试重新连接:" + String(count));
            delay(5000);
            if(first_boot && (count > RECONNECT_TIMES))
                goto __bambu_connect_fail__;
            if(first_boot){
                goto __bambu_reconnect__;
            }

        }
        if (count < RECONNECT_TIMES)
            return;
__bambu_connect_fail__:
        DBG_BAMBU("拓竹连接超时!请检查配置");
        DBG_BAMBU("拓竹ip地址["+String(bambu.cfg->mqtt_broker)+"]");
        DBG_BAMBU("拓竹序列号["+String(bambu.cfg->device_serial)+"]");
        DBG_BAMBU("拓竹访问码["+String(bambu.cfg->mqtt_password)+"]");
        motor.ops->keepLow(); //复位前锁住电机IO防止转动
        ESP.restart();
        while(1);
    }
}

static void debugCtrlBambuMQTT(String command) {
    String *msg;
    if(command == "ledON"){
        msg = &bambu.comm->ledON;
    }else if(command == "ledOFF"){
        msg = &bambu.comm->ledOFF;
    }else if(command == "resume"){
        msg = &bambu.comm->resume;
    }else if(command == "unload"){
        msg = &bambu.comm->unload;
    }else if(command == "load"){
        msg = &bambu.comm->load;
    }else if(command == "done"){
        msg = &bambu.comm->done;
    }else if(command == "stop"){
        msg = &bambu.comm->pause;
    }else{
        return;
    }
    bambuClient.publish(bambu.cfg->topic_publish.c_str(),msg->c_str());
    DBG_BAMBU(("MQTT Send>>" + command));
}

static void mqtt_get_data(byte* payload, unsigned int length){
    static bool first_boot = true;
    JsonDocument data;
    deserializeJson(data, payload, length);
    bambu.data->sequenceId       = (data)["print"]["sequence_id"].as<String>();
    bambu.data->amsStatus        = (data)["print"]["ams_status"].as<String>();
    bambu.data->printError       = (data)["print"]["print_error"].as<String>();
    bambu.data->hwSwitchState    = (data)["print"]["hw_switch_state"].as<String>();
    bambu.data->gcodeState       = (data)["print"]["gcode_state"].as<String>();
    bambu.data->mcPercent        = (data)["print"]["mc_percent"].as<String>();
    bambu.data->mcRemainingTime  = (data)["print"]["mc_remaining_time"].as<String>();
    if(bambu.data->mcPercent.toInt() >= 110)
        bambu.data->nextFilament = String(bambu.data->mcPercent.toInt() - 110);
    if(bambu.data->gcodeState == "PAUSE" || first_boot){
        first_boot = false;
        data  = my_fs.ops->read("/data.json");
        bambu.data->lastFilament  = (data)["lastFilament"].as<String>();
    }
}

static void printdata(void){
    if (!(bambu.data->amsStatus     != "null" ||
        bambu.data->hwSwitchState   != "null" ||
        bambu.data->gcodeState      != "null" ||
        bambu.data->mcRemainingTime != "null"))
        return;
    Serial.println("=-=-=-=-=-=-=-=-=-=-=");
    Serial.println("sequenceId:      " + bambu.data->sequenceId);
    Serial.println("amsStatus:       " + bambu.data->amsStatus);
    Serial.println("hwSwitchState:   " + bambu.data->hwSwitchState);
    Serial.println("gcodeState:      " + bambu.data->gcodeState);
    Serial.println("mcPercent:       " + bambu.data->mcPercent);
    Serial.println("mcRemainingTime: " + bambu.data->mcRemainingTime);
    Serial.println("printError:      " + bambu.data->printError);
    Serial.println("nextFilament:    " + bambu.data->nextFilament);
    Serial.println("lastFilament:    " + bambu.data->lastFilament);
    Serial.println("+=+=+=+=+=+=+=+=+=+=+");
}

static void work(void){
    static union RunningState runState = {0};
    static uint8_t err_load_count = 0; //缠料报错5次之后不再重试打印
    static uint8_t heartbeat = 0;
    heartbeat++;
    if(heartbeat > 15){
        heartbeat = 0;
        DBG_BAMBU("=========当前通道: "+String(bambu.data->lastFilament.toInt() + 1)+"=========\n");
    }
    //换料申请
    if((bambu.data->gcodeState == "PAUSE" && bambu.data->nextFilament.toInt() >= 0) && err_load_count <= 5){
        //触发缠料检查
        if(bambu.data->printError == "318734352" && !runState.bit.err_load){
            DBG_BAMBU("缠料检查\n");
            err_load_count++;
            runState.bit.err_load = true;
            //电机向前推动50ms 解除齿轮卡死状态
            motor.ops->run_ms(motor.mt[bambu.data->lastFilament.toInt()],DIR_LOAD,HOMEING_TIME);
            bambu.Client->publish(bambu.cfg->topic_publish.c_str(), bambu.comm->resume.c_str());
        }
        //申请通道与当前通道一致，无需换料
        else if(bambu.data->nextFilament == bambu.data->lastFilament){
            if(bambu.data->printError == "318734343"){
                DBG_BAMBU("缠料处理,重试打印\n");
                bambu.Client->publish(bambu.cfg->topic_publish.c_str(), bambu.comm->done.c_str());
                //检测到缠料时会自动切断料,恢复打印后ams送500ms料,确保重新上料成功
                motor.ops->run_ms(motor.mt[bambu.data->lastFilament.toInt()],DIR_LOAD,500);
                //===回中===
                motor.ops->run_ms(motor.mt[bambu.data->lastFilament.toInt()],DIR_UNLOAD,HOMEING_TIME);
            }
            else{
                DBG_BAMBU("通道一致,无需换料\n");
                bambu.Client->publish(bambu.cfg->topic_publish.c_str(), bambu.comm->resume.c_str());
            }
            bambu.data->gcodeState = "null";
            delay(1500);
        }
        //需要换料
        else{
        //===退料===
            if(!runState.bit.unload){
                DBG_BAMBU("开始退料\n");
                runState.bit.unload = true;
                bambu.Client->publish(bambu.cfg->topic_publish.c_str(), bambu.comm->unload.c_str());
            }
            //通知电机退料
            if(bambu.data->printError == "318750723" && !runState.bit.motor){
                DBG_BAMBU("退料通道:" + String(bambu.data->lastFilament.toInt() + 1) + "\n");
                runState.bit.motor = true;
                //===驱动通道电机退料===
                motor.ops->unload(motor.mt[bambu.data->lastFilament.toInt()]);
            }
            //拔料超时，重试退料
            if(bambu.data->printError == "318734339" && runState.bit.motor){
                DBG_BAMBU("重试退料 通道:" + String(bambu.data->lastFilament.toInt() + 1) + "\n");
                bambu.Client->publish(bambu.cfg->topic_publish.c_str(), bambu.comm->resume.c_str());
                delay(1000);
                //===驱动通道电机退料===
                motor.ops->unload(motor.mt[bambu.data->lastFilament.toInt()]);
            }
            //退料完成
            if(bambu.data->amsStatus == "0" && !runState.bit.unload_done){
                DBG_BAMBU("退料成功\n");
                runState.bit.unload_done = true;
                //===停电机===
                motor.ops->stop(motor.mt[bambu.data->lastFilament.toInt()]);
                //===回中===
                motor.ops->run_ms(motor.mt[bambu.data->lastFilament.toInt()],DIR_LOAD,HOMEING_TIME);
            }
        //===上料===
            if(runState.bit.unload_done && !runState.bit.load){
                DBG_BAMBU("开始上料\n");
                runState.bit.load = true;
                runState.bit.motor = 0;
                bambu.Client->publish(bambu.cfg->topic_publish.c_str(), bambu.comm->load.c_str());
            }
            //通知电机送料
            if(bambu.data->printError == "318750726" && !runState.bit.motor){
                DBG_BAMBU("上料通道" + String(bambu.data->nextFilament.toInt() + 1) + "\n");
                runState.bit.motor = true;
                //===驱动通道电机上料===
                motor.ops->load(motor.mt[bambu.data->nextFilament.toInt()]);
            }
            //送料到位
            if(bambu.data->amsStatus == "262" && !runState.bit.load_done){
                DBG_BAMBU("送料到位\n");
                runState.bit.load_done = true;
                //===停电机===
                motor.ops->stop(motor.mt[bambu.data->nextFilament.toInt()]);
                //===回中===
                motor.ops->run_ms(motor.mt[bambu.data->lastFilament.toInt()],DIR_UNLOAD,HOMEING_TIME);
            }
            //提示观察喷嘴(需要点击确认)
            if((bambu.data->printError == "318734343" && bambu.data->hwSwitchState == "1") && runState.bit.load_done){
                DBG_BAMBU("观察喷嘴\n");
                bambu.Client->publish(bambu.cfg->topic_publish.c_str(), bambu.comm->done.c_str());
                bambu.data->printError = "null";
                delay(1000);
            }
            //上料完成
            if(bambu.data->amsStatus == "768"){
                DBG_BAMBU("上料成功\n");
                JsonDocument data;
                data["lastFilament"] =  bambu.data->nextFilament;
                bambu.data->lastFilament = bambu.data->nextFilament;
                my_fs.ops->write(data,"/data.json");
                runState = {0};
                bambu.Client->publish(bambu.cfg->topic_publish.c_str(), bambu.comm->resume.c_str());
                bambu.data->amsStatus = "null";
                bambu.data->gcodeState = "null";
                delay(1500);
            }
        }
    }
}



static BAMBU_OPS ops = {
    .connect = connectBambuMQTT,
    .debugCtrl = debugCtrlBambuMQTT,
    .getdata = mqtt_get_data,
    .printdata = printdata,
    .work = work,
};

BAMBU bambu = {
    .WifiClient = &bambuWifiClient,
    .Client = &bambuClient,
    .comm = &command,
    .data = &data,
    .cfg = &cfg,
    .ops = &ops,
};
